#include <ros/ros.h>
#include <rosbag/bag.h>
#include <sensor_msgs/Imu.h>
#include <stdio.h>
#include <time.h>

#include <cmath>
#include <iostream>
#include <string>
#include <vector>

using namespace std;

#define BASE_TIME 1609307878

struct imu_data {
  double sec;
  double gx;
  double gy;
  double gz;
  double ax;
  double ay;
  double az;
  double mx;
  double my;
  double mz;
  // double ox;
  // double oy;
  // double oz;
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "imu_node");
  ros::NodeHandle n("~");

  rosbag::Bag bag;
  bag.open("kalibr_imu.bag", 1U);

  printf("read dataPath: %s\n", argv[1]);
  string dataPath = argv[1];
  FILE* file;

  // Step 1 record  imu data into kalibr bag
  // Step 1.1 read imu data
  cout << dataPath + "/imu.txt" << endl;
  file = std::fopen((dataPath + "/imu.txt").c_str(), "r");
  if (file == NULL) {
    printf("cannot find file: %s \n", dataPath.c_str());
    ROS_BREAK();
    return 0;
  }
  vector<imu_data> imuList;
  char string_Temp[60];
  double acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, m_x, m_y, m_z;
  long nanosec;
  ROS_INFO("reading head ");
  for (int i = 0; i < 13; i++) {
    fscanf(file, "%s", &string_Temp);
  }
  ROS_INFO("reading data");
  for (int i = 0; i < 13; i++) {
    fscanf(file, "%s", &string_Temp);
  }
  while (fscanf(file, "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", &nanosec, &gyro_x, &gyro_y, &gyro_z, &acc_x, &acc_y, &acc_z, &m_x, &m_y, &m_z) != EOF) {
    double sec = nanosec / 1000000000.0;
    imu_data data;
    data.sec = sec;

    data.gx = gyro_z;
    data.gy = gyro_x;
    data.gz = gyro_y;

    data.ax = acc_z;
    data.ay = acc_x;
    data.az = acc_y;

    data.mx = m_z;
    data.my = m_x;
    data.mz = m_y;
    imuList.push_back(data);
  }
  std::fclose(file);

  // Step 1.2 write imu data to bag
  // imu frequency is 1000Hz,down to 100Hz
  for (int i = 1; i < imuList.size(); i += 10) {
    if (ros::ok()) {
      printf("\nprocess imu %d   size:%d\n", (int)i, (int)imuList.size());
      double time = BASE_TIME + imuList[i].sec;
      sensor_msgs::Imu imu_msg;
      imu_msg.header.stamp = ros::Time(time);
      imu_msg.linear_acceleration.x = imuList[i].ax;
      imu_msg.linear_acceleration.y = imuList[i].ay;
      imu_msg.linear_acceleration.z = imuList[i].az;

      imu_msg.angular_velocity.x = imuList[i].gx;
      imu_msg.angular_velocity.y = imuList[i].gy;
      imu_msg.angular_velocity.z = imuList[i].gz;
      bag.write("imu", imu_msg.header.stamp, imu_msg);
    } else {
      break;
    }
  }

  // TODO: record image data into kalibr bag
  // Step 2 record image data into kalibr bag
  // Step 2.1 read image data

  // Step 2.2 write imu data to bag
  
  //   for (int i = 0; i < imageTimeList.size(); i++) {
  //     if (ros::ok()) {
  //       printf("\nprocess image %d\n", i);
  //       stringstream ss;
  //       ss << imageTimeList[i];
  //       // cout << ss.str() <<endl;
  //       //交换左右目
  //       leftImagePath = dataPath + "/cam1/data/" + ss.str() + ".png";
  //       rightImagePath = dataPath + "/cam0/data/" + ss.str() + ".png";

  //       double time = BASE_TIME + imageTimeList[i] / 1000000000.0;

  //       imleft_src = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE);
  //       //翻转图像
  //       // imLeft = imleft_src.clone();
  //       cv::flip(imleft_src, imLeft, -1);
  //       // ROS_INFO("flip!");
  //       sensor_msgs::ImagePtr imLeftMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imLeft).toImageMsg();
  //       imLeftMsg->header.stamp = ros::Time(time);
  //       bag.write("left", imLeftMsg->header.stamp, imLeftMsg);

  //       imright_src = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE);
  //       // imRight = imright_src.clone();
  //       //翻转图像
  //       cv::flip(imright_src, imRight, -1);
  //       sensor_msgs::ImagePtr imRightMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imRight).toImageMsg();
  //       imRightMsg->header.stamp = ros::Time(time);
  //       bag.write("right", imRightMsg->header.stamp, imRightMsg);
  //     } else
  //       break;
  //   }
  
  // return 0;
}